#include "PID_Angle_IR_Speed.h"
#include <math.h>
// #include <stdlib.h>
#define MAX_I_SUM 500        // 积分项最大累积值
#define PID_DEADBAND 1        // 死区范围（误差小于此值认为为0）
extern PID_Struct Motor_PID_left;
extern PID_Struct Motor_PID_Right;

void Motor_Angle_PID_Init_Left(void)
{
	Motor_PID_left.Kp = 8;			
	Motor_PID_left.Ti = 5;		
	Motor_PID_left.Td = 0.0001;			
	
	Motor_PID_left.SetValue =  25;	
	Motor_PID_left.T = 2; 			
	
	Motor_PID_left.PWMCycle = 1000;	
	Motor_PID_left.Out0 = 1;			
}

void Motor_Angle_PID_Cal_Left(void)
{
	float Ki, Kd, DletaK;

	if (Motor_PID_left.PIDCounter < Motor_PID_left.T) return;

	// --- 计算误差 ---
	Motor_PID_left.EK = Motor_PID_left.SetValue - Motor_PID_left.GetValue;

	// --- 死区 ---
	if (abs(Motor_PID_left.EK) < PID_DEADBAND)
		Motor_PID_left.EK = 0;

	// --- 比例项 ---
	Motor_PID_left.Pout = Motor_PID_left.EK * Motor_PID_left.Kp;

	// --- 积分项 ---
	Motor_PID_left.SigmaEK += Motor_PID_left.EK;

	// 限制积分项累加大小
	if (Motor_PID_left.SigmaEK > MAX_I_SUM)
		Motor_PID_left.SigmaEK = MAX_I_SUM;
	else if (Motor_PID_left.SigmaEK < -MAX_I_SUM)
		Motor_PID_left.SigmaEK = -MAX_I_SUM;

	Ki = Motor_PID_left.Kp * Motor_PID_left.T / Motor_PID_left.Ti;
	Motor_PID_left.Iout = Ki * Motor_PID_left.SigmaEK;

	// --- 微分项 ---
	DletaK = Motor_PID_left.EK - Motor_PID_left.EK_1;
	Kd = Motor_PID_left.Kp * Motor_PID_left.Td / Motor_PID_left.T;
	Motor_PID_left.Dout = DletaK * Kd;

	// --- PID 输出汇总 ---
	Motor_PID_left.Out = Motor_PID_left.Pout + Motor_PID_left.Iout + Motor_PID_left.Dout;

	// --- 限制输出 ---
	if (Motor_PID_left.Out > Motor_PID_left.PWMCycle)
		Motor_PID_left.Out = Motor_PID_left.PWMCycle;
	else if (Motor_PID_left.Out < Motor_PID_left.Out0)
		Motor_PID_left.Out = Motor_PID_left.Out0 = 1;

	// --- 保存当前误差、清零计数器 ---
	Motor_PID_left.EK_1 = Motor_PID_left.EK;
	Motor_PID_left.PIDCounter = 0;
}

void Motor_Angle_PID_Init_Right(void)
{
	Motor_PID_Right.Kp = 8;			
	Motor_PID_Right.Ti = 5;		
	Motor_PID_Right.Td = 0.0001;			
	
	Motor_PID_Right.SetValue =  25;	
	Motor_PID_Right.T = 2; 			
	
	Motor_PID_Right.PWMCycle = 1000;	
	Motor_PID_Right.Out0 = 1;			
}	

void Motor_Angle_PID_Cal_Right(void)
{
	float Ki, Kd, DletaK;

	if (Motor_PID_Right.PIDCounter < Motor_PID_Right.T) return;

	// --- 计算误差 ---
	Motor_PID_Right.EK = Motor_PID_Right.SetValue - Motor_PID_Right.GetValue;

	// --- 死区 ---
	if (abs(Motor_PID_Right.EK) < PID_DEADBAND)
		Motor_PID_Right.EK = 0;

	// --- 比例项 ---
	Motor_PID_Right.Pout = Motor_PID_Right.EK * Motor_PID_Right.Kp;

	// --- 积分项 ---
	Motor_PID_Right.SigmaEK += Motor_PID_Right.EK;

	// 限制积分项累加大小
	if (Motor_PID_Right.SigmaEK > MAX_I_SUM)
		Motor_PID_Right.SigmaEK = MAX_I_SUM;
	else if (Motor_PID_Right.SigmaEK < -MAX_I_SUM)
		Motor_PID_Right.SigmaEK = -MAX_I_SUM;

	Ki = Motor_PID_Right.Kp * Motor_PID_Right.T / Motor_PID_Right.Ti;
	Motor_PID_Right.Iout = Ki * Motor_PID_Right.SigmaEK;

	// --- 微分项 ---
	DletaK = Motor_PID_Right.EK - Motor_PID_Right.EK_1;
	Kd = Motor_PID_Right.Kp * Motor_PID_Right.Td / Motor_PID_Right.T;
	Motor_PID_Right.Dout = DletaK * Kd;

	// --- PID 输出汇总 ---
	Motor_PID_Right.Out = Motor_PID_Right.Pout + Motor_PID_Right.Iout + Motor_PID_Right.Dout;

	// --- 限制输出 ---
	if (Motor_PID_Right.Out > Motor_PID_Right.PWMCycle)
		Motor_PID_Right.Out = Motor_PID_Right.PWMCycle;
	else if (Motor_PID_Right.Out < Motor_PID_Right.Out0)
		Motor_PID_Right.Out = Motor_PID_Right.Out0 = 1;

	// --- 保存当前误差、清零计数器 ---
	Motor_PID_Right.EK_1 = Motor_PID_Right.EK;
	Motor_PID_Right.PIDCounter = 0;
}






// // 初始化 PID 控制器参数
// void PID_SpeedL_Init(PIDController *pid, float kp, float ki, float kd, float limit)
// {
//     pid->Kp = kp;
//     pid->Ki = ki;
//     pid->Kd = kd;
//     pid->integral = 0.0;
//     pid->prevError = 0.0;
//     pid->outputLimit = limit;
//     pid->lostCounter = 0;
//     pid->lastValidErr = 0.0;
// }
// // PID 控制器计算输出
// float PID_SpeedL_Compute(PIDController *pid, float setpoint, float currentValue)
// {
//     float error = setpoint - currentValue;
//     pid->integral += error;

//     // 限制积分项防止积分饱和
//     if (pid->integral > 100.0) pid->integral = 100.0;
//     if (pid->integral < -100.0) pid->integral = -100.0;

//     float derivative = error - pid->prevError;
//     pid->prevError = error;

//     float output = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);

//     // 限制输出范围
//     if (output > pid->outputLimit) output = pid->outputLimit;
//     if (output < -pid->outputLimit) output = -pid->outputLimit;

//     return output;
// }

// // 初始化 PID 控制器参数
// void PID_SpeedR_Init(PIDController *pid, float kp, float ki, float kd, float limit)
// {
//     pid->Kp = kp;
//     pid->Ki = ki;
//     pid->Kd = kd;
//     pid->integral = 0.0;
//     pid->prevError = 0.0;
//     pid->outputLimit = limit;
//     pid->lostCounter = 0;
//     pid->lastValidErr = 0.0;
// }
// // PID 控制器计算输出
// float PID_SpeedR_Compute(PIDController *pid, float setpoint, float currentValue)
// {
//     float error = setpoint - currentValue;
//     pid->integral += error;

//     // 限制积分项防止积分饱和
//     if (pid->integral > 100.0) pid->integral = 100.0;
//     if (pid->integral < -100.0) pid->integral = -100.0;

//     float derivative = error - pid->prevError;
//     pid->prevError = error;

//     float output = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);

//     // 限制输出范围
//     if (output > pid->outputLimit) output = pid->outputLimit;
//     if (output < -pid->outputLimit) output = -pid->outputLimit;

//     return output;
// }


// 初始化 PID 控制器参数
void PID_Angle_Init(PIDController *pid, float kp, float ki, float kd, float limit)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral = 0.0;
    pid->prevError = 0.0;
    pid->outputLimit = limit;
    pid->lostCounter = 0;
    pid->lastValidErr = 0.0;
}

// PID 控制器计算输出
float PID_Angle_Compute(PIDController *pid, float setpoint, float currentValue)
{
    float error = setpoint - currentValue;
    pid->integral += error;

    // 限制积分项防止积分饱和
    if (pid->integral > 100.0) pid->integral = 100.0;
    if (pid->integral < -100.0) pid->integral = -100.0;

    float derivative = error - pid->prevError;
    pid->prevError = error;

    float output = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);

    // 限制输出范围
    if (output > pid->outputLimit) output = pid->outputLimit;
    if (output < -pid->outputLimit) output = -pid->outputLimit;

    return output;
}

// 初始化 PID 控制器参数
void PID_IR_Init(PIDController *pid, float kp, float ki, float kd, float limit)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral = 0.0;
    pid->prevError = 0.0;
    pid->outputLimit = limit;
    pid->lostCounter = 0;
    pid->lastValidErr = 0.0;
}

// PID 控制器计算输出
float PID_IR_Compute(PIDController *pid, float setpoint, float currentValue)
{
    float error = setpoint - currentValue;
    pid->integral += error;

    // 限制积分项防止积分饱和
    if (pid->integral > 100.0) pid->integral = 100.0;
    if (pid->integral < -100.0) pid->integral = -100.0;

    float derivative = error - pid->prevError;
    pid->prevError = error;

    float output = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);

    // 限制输出范围
    if (output > pid->outputLimit) output = pid->outputLimit;
    if (output < -pid->outputLimit) output = -pid->outputLimit;

    return output;
}
